program number3
use analyse_lattice
implicit none
integer no,np,neq
type(real_8) y(nvd)   ! nvd=3
type(normalform) normal
type(damap) map
type(taylor) x2ave
real(dp) fix(nvd),prec,CORRECTOR(10)
real radius,res(3)
integer i,pos,k,lat
type(pbresonance) H
type(vecresonance) F
type(TAYLOR) R(2),sex(6)
type(GMAP) G
LOGICAL MORE
prec=1.d-10

open(unit=20,file="pref.txt")
read(20,*) lat
read(20,*)  radius
if(lat==36) then
 call make_lattice("lattice36_100_20.txt")
elseif(lat==335) then
 call make_lattice("lattice335_100_20.txt")
elseif(lat==37) then
 call make_lattice("lattice37_100_20.txt")
elseif(lat==39) then
 call make_lattice("lattice39_100_20.txt")
endif
close(20)
! We are allowed 3 correctors qf1,qd2,qf3,qd4,qf5,qd6,qf7,qd8

!First thing we can do: try to reduce the driving term of 4nux
! using octupoles terms 

100 call locate_set_knobs(2)




open(unit=20,file="results_normal_form.txt")

! Compute One-turn map and normalize it including correctors


 pos=1

fix=0.d0;

knob=.false.
call find_orbit_with_fpp(lattice,pos,fix)
knob=.true.

no=5;np=10;

call init(no,1,np,0)
call alloc(y); call alloc(normal); call alloc(map);CALL ALLOC(H); CALL ALLOC(F);CALL ALLOC(R)
call alloc(sex);

do i=1,2
y(i)=fix(i)+(1.d0.mono.i)
enddo

call track(y,lattice,pos)


map=y

normal%NRES=2
normal%m(1,1)=3
normal%m(1,2)=6
normal=map

 
 write(20,*) "   "


H=NORMAL%normal%PB
F=NORMAL%normal%NONLINEAR



 write(20,*) "   "
 write(20,*) " Normal form "


call print(H,20,prec)


SEX(1)=F%SIN%V(2)<='20'
SEX(2)=F%COS%V(2)<='20'
SEX(3)=F%SIN%V(2)<='31'
SEX(4)=F%COS%V(2)<='31'
SEX(5)=F%SIN%V(2)<='50'
SEX(6)=F%COS%V(2)<='50'

res(1)=sqrt((sex(1).sub.'0')**2+(sex(2).sub.'0')**2)
res(2)=sqrt((sex(3).sub.'0')**2+(sex(4).sub.'0')**2)
res(3)=sqrt((sex(5).sub.'0')**2+(sex(6).sub.'0')**2)

write(6,*) " resonance terms "

write(6,*) res
write(6,*) " resonance terms scaled by radius =",radius

write(6,*) res(1),res(2)*radius**2,res(3)*radius**4

OPEN(UNIT=21,FILE='TEMP')
 DO I=1,6; CALL PRINT(SEX(I),21);ENDDO;
CLOSE(21)

call kill(y); call kill(normal); call kill(map);CALL kill(H); CALL kill(F);CALL kill(R)
call kill(sex);

neq=2
CALL INIT(2,np+neq)
CALL ALLOC(G); CALL ALLOC(R);CALL ALLOC(SEX)

OPEN(UNIT=21,FILE='TEMP')
 DO I=1,neq; CALL read(SEX(I),21);ENDDO;
CLOSE(21)



DO I=1,np
  G%V(I)= (1.D0.MONO.I)
  DO K=1,neq
   G%V(I)=G%V(I)+ (1.D0.MONO.(np+K))*(SEX(K).D.I)
 ENDDO
ENDDO

 DO K=1,neq
G%V(np+K)=SEX(K)
ENDDO

G=G.OO.(-1)

DO I=1,np
CORRECTOR(I)=G%V(I).SUB.'0'
ENDDO
WRITE(6,*) CORRECTOR
CALL locate_POWER_knobs(CORRECTOR)

CALL KILL(G); CALL KILL(R); CALL KILL(SEX);
CLOSE(20)
WRITE(6,*) " MORE ITERATIONS "
READ(5,*) MORE

IF(MORE) GOTO 100
if(lat==36) then
call write_lattice("lattice36_100_20_c.txt")
elseif(lat==37) then
call write_lattice("lattice37_100_20_c.txt")
elseif(lat==39) then
call write_lattice("lattice39_100_20_c.txt")
elseif(lat==335) then
call write_lattice("lattice335_100_20_c.txt")

endif

end program number3

subroutine locate_set_knobs(npara)
use analyse_lattice
implicit none
integer i,npara,k,BN
k=0
BN=3
do i=1,lattice%n

select case(lattice%magnet(i)%name(1:4))

case('QF1 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+1
case('QD2 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+2
case('QF3 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+3
case('QD4 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+4
case('QF5 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+5
case('QD6 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+6
case('QF7 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+7
case('QD8 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+8
case('QF9 ')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+9
case('QD10')
lattice%magnet(i)%k(BN)%kind=3
lattice%magnet(i)%k(BN)%i=npara+10
case default
k=k+1
end select
enddo 

write(6,*)lattice%n-k, " knobs found "
end subroutine locate_set_knobs

subroutine locate_POWER_knobs(C)
use analyse_lattice
implicit none
REAL(DP) C(10)
integer i,npara,k,BN
k=0
BN=3
do i=1,lattice%n

select case(lattice%magnet(i)%name(1:4))

case('QF1 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(1)+lattice%magnet(i)%k(BN)%R
case('QD2 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(2)+lattice%magnet(i)%k(BN)%R
case('QF3 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(3)+lattice%magnet(i)%k(BN)%R
case('QD4 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(4)+lattice%magnet(i)%k(BN)%R
case('QF5 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(5)+lattice%magnet(i)%k(BN)%R
case('QD6 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(6)+lattice%magnet(i)%k(BN)%R
case('QF7 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(7)+lattice%magnet(i)%k(BN)%R
case('QD8 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(8)+lattice%magnet(i)%k(BN)%R
case('QF9 ')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(9)+lattice%magnet(i)%k(BN)%R
case('QD10')
lattice%magnet(i)%k(BN)%kind=1
lattice%magnet(i)%k(BN)%R=C(10)+lattice%magnet(i)%k(BN)%R
case default
k=k+1
end select
enddo 

write(6,*)lattice%n-k, " knobs found "
end subroutine locate_POWER_knobs
